﻿using TPXSDK.NET.Share;

namespace TPXSDK.NET.GIS;

public class MapProjection
{
    #region 成员变量
    private double _ref_lat;
    private double _ref_lon;
    private double _ref_sin_lat;
    private double _ref_cos_lat;
    private double CONSTANTS_RADIUS_OF_EARTH = 6371000;// meters (m)

    #region WGS84 椭球参数
    private const double A = 6378137.0;             // 长半轴
    private const double F = 1.0 / 298.257223563;   // 扁率
    private const double E2 = F * (2 - F);          // 第一偏心率平方
    #endregion

    #endregion

    public void InitReference(double lat_0, double lon_0)
    {
        _ref_lat = DataUtils.Deg2Rad(lat_0);
        _ref_lon = DataUtils.Deg2Rad(lon_0);
        _ref_sin_lat = Math.Sin(_ref_lat);
        _ref_cos_lat = Math.Cos(_ref_lat);
    }

    public RelPos Project(double lat, double lon)
    {
        var lat_rad = DataUtils.Deg2Rad(lat);
        var lon_rad = DataUtils.Deg2Rad(lon);

        var sin_lat = Math.Sin(lat_rad);
        var cos_lat = Math.Cos(lat_rad);

        var cos_d_lon = Math.Cos(lon_rad - _ref_lon);

        var arg = DataUtils.Constrain(_ref_sin_lat * sin_lat +
                                      _ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0);
        var c = Math.Acos(arg);

        double k = 1.0;

        if (Math.Abs(c) > 0)
        {
            k = c / Math.Sin(c);
        }

        return new RelPos(k * (_ref_cos_lat * sin_lat - _ref_sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH,
                          k * cos_lat * Math.Sin(lon_rad - _ref_lon) * CONSTANTS_RADIUS_OF_EARTH);
    }

    public AbsPos Reproject(double x, double y)
    {
        var x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
        var y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
        var c = Math.Sqrt(x_rad * x_rad + y_rad * y_rad);

        var pos = new AbsPos();
        if (Math.Abs(c) > 0)
        {
            var sin_c = Math.Sin(c);
            var cos_c = Math.Cos(c);

            var lat_rad = Math.Asin(cos_c * _ref_sin_lat + x_rad * sin_c * _ref_cos_lat / c);
            var lon_rad = _ref_lon + Math.Atan2(y_rad * sin_c, c * _ref_cos_lat * cos_c - x_rad * _ref_sin_lat * sin_c);

            pos.Lat = DataUtils.Rad2Deg(lat_rad);
            pos.Lng = DataUtils.Rad2Deg(lon_rad);
        }
        else
        {
            pos.Lat = DataUtils.Rad2Deg(_ref_lat);
            pos.Lng = DataUtils.Rad2Deg(_ref_lon);
        }
        return pos;
    }
}
